#include <string>
#include <vector>
#include <iostream>
#include <cmath>

#include "cartographer/common/configuration_file_resolver.h"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/port.h"
#include "cartographer_ros/node_constants.h"
#include "cartographer_ros/ros_log_sink.h"
#include "cartographer_ros/trajectory_options.h"
#include "cartographer_ros_msgs/StartTrajectory.h"
#include "cartographer_ros_msgs/StatusCode.h"
#include "cartographer_ros_msgs/TrajectoryOptions.h"
#include "gflags/gflags.h"
#include "ros/ros.h"

// ed: header added
#include "tf/tf.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "cartographer_ros_msgs/FinishTrajectory.h"
#include "cartographer/io/proto_stream.h"
#include "cartographer_ros/submap.h"
#include "cartographer/mapping/map_builder_interface.h"
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
#include "cartographer_ros/msg_conversion.h"
#include "cartographer/mapping/map_builder.h"
#include "cartographer_ros/node.h"
#include "cartographer_ros/node_options.h"

#include "absl/memory/memory.h"

DEFINE_string(configuration_directory, "",
              "First directory in which configuration files are searched, "
              "second is always the Cartographer installation to allow "
              "including files from there.");
DEFINE_string(configuration_basename, "",
              "Basename, i.e. not containing any directory prefix, of the "
              "configuration file.");

DEFINE_string(initial_pose, "", "Starting pose of a new trajectory");

// ed: pbstream filename to get last trajectory's position(x,y,z) value 
DEFINE_string(load_state_filename, "",
              "Filename of a pbstream to draw a map from.");

namespace cartographer_ros {
namespace {

TrajectoryOptions LoadOptions() {
  auto file_resolver = absl::make_unique<
      cartographer::common::ConfigurationFileResolver>(
      std::vector<std::string>{FLAGS_configuration_directory});
  const std::string code =
      file_resolver->GetFileContentOrDie(FLAGS_configuration_basename);
  auto lua_parameter_dictionary =
      cartographer::common::LuaParameterDictionary::NonReferenceCounted(
          code, std::move(file_resolver));
  if (!FLAGS_initial_pose.empty()) {
    auto initial_trajectory_pose_file_resolver =
        absl::make_unique<
            cartographer::common::ConfigurationFileResolver>(
            std::vector<std::string>{FLAGS_configuration_directory});
    auto initial_trajectory_pose =
        cartographer::common::LuaParameterDictionary::NonReferenceCounted(
            "return " + FLAGS_initial_pose,
            std::move(initial_trajectory_pose_file_resolver));
    return CreateTrajectoryOptions(lua_parameter_dictionary.get(),
                                   initial_trajectory_pose.get());
  } else {
    return CreateTrajectoryOptions(lua_parameter_dictionary.get());
  }
}

bool Run() {
  ros::NodeHandle node_handle;
  ros::ServiceClient client =
      node_handle.serviceClient<cartographer_ros_msgs::StartTrajectory>(
          kStartTrajectoryServiceName);
  cartographer_ros_msgs::StartTrajectory srv;
  srv.request.options = ToRosMessage(LoadOptions());
  srv.request.topics.laser_scan_topic = node_handle.resolveName(
      kLaserScanTopic, true // apply topic remapping 
      );
  srv.request.topics.multi_echo_laser_scan_topic =
      node_handle.resolveName(kMultiEchoLaserScanTopic, true);
  srv.request.topics.point_cloud2_topic =
      node_handle.resolveName(kPointCloud2Topic, true);
  srv.request.topics.imu_topic = node_handle.resolveName(kImuTopic, true);
  srv.request.topics.odometry_topic =
      node_handle.resolveName(kOdometryTopic, true);

  if (!client.call(srv)) {
    LOG(ERROR) << "Failed to call " << kStartTrajectoryServiceName << ".";
    return false;
  }
  if (srv.response.status.code != cartographer_ros_msgs::StatusCode::OK) {
    LOG(ERROR) << "Error starting trajectory - message: '"
               << srv.response.status.message
               << "' (status code: " << std::to_string(srv.response.status.code)
               << ").";
    return false;
  }
  LOG(INFO) << "Started trajectory " << srv.response.trajectory_id;
  return true;
}

}  // namespace
}  // namespace cartographer_ros

void recordInitialPose() {
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener tfl(buffer);

    ros::Time time = ros::Time(0);
    ros::Duration timeout(3.0);

    geometry_msgs::TransformStamped transform;
    transform = buffer.lookupTransform("map", "base_link", time, timeout);

    tf2::Stamped<tf2::Transform> tf2;
    tf2::convert(transform, tf2);
 
  ::cartographer::io::ProtoStreamReader reader(FLAGS_load_state_filename);
  cartographer_ros::NodeOptions node_options;
  cartographer_ros::TrajectoryOptions trajectory_options;
  std::tie(node_options, trajectory_options) =
      cartographer_ros::LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
  auto map_builder =
      absl::make_unique<cartographer::mapping::MapBuilder>(
          node_options.map_builder_options);
  
  // ed: load pbstream 
  map_builder->LoadState(&reader, true);

  // ed: get TrajectoryNodePose 
  const auto node_poses = map_builder->pose_graph()->GetTrajectoryNodePoses();
  
  double pbstream_x = std::prev(node_poses.EndOfTrajectory(0))->data.global_pose.translation().x();
  double pbstream_y = std::prev(node_poses.EndOfTrajectory(0))->data.global_pose.translation().y();
  double pbstream_z = std::prev(node_poses.EndOfTrajectory(0))->data.global_pose.translation().z();
  double pbstream_quat_x = (std::prev(node_poses.EndOfTrajectory(0)))->data.global_pose.rotation().x();
  double pbstream_quat_y = (std::prev(node_poses.EndOfTrajectory(0)))->data.global_pose.rotation().y();
  double pbstream_quat_z = (std::prev(node_poses.EndOfTrajectory(0)))->data.global_pose.rotation().z();
  double pbstream_quat_w = (std::prev(node_poses.EndOfTrajectory(0)))->data.global_pose.rotation().w();

  tf::Transform map_tf;
  tf::Transform EOT_tf;
  tf::Transform output_tf;

  map_tf.setOrigin(tf::Vector3(transform.transform.translation.x,
                              transform.transform.translation.y,
                              transform.transform.translation.z));
  map_tf.setRotation(tf::Quaternion(transform.transform.rotation.x,
                                   transform.transform.rotation.y,
                                   transform.transform.rotation.z,
                                   transform.transform.rotation.w));

  EOT_tf.setOrigin(tf::Vector3(pbstream_x,
                               pbstream_y,
                               pbstream_z));
  EOT_tf.setRotation(tf::Quaternion(pbstream_quat_x,
                                    pbstream_quat_y,
                                    pbstream_quat_z,
                                    pbstream_quat_w));

  output_tf = EOT_tf.inverse() * map_tf;

  // ed: Quaternion to RPY
  tf::Quaternion q(
      output_tf.getRotation().x(),
      output_tf.getRotation().y(),
      output_tf.getRotation().z(),
      output_tf.getRotation().w());
  tf::Matrix3x3 m(q);

  double roll, pitch, yaw;
  m.getRPY(roll, pitch, yaw);

  int to_trajectory_id = 0;   // ed: offline map's trajectory

    // std::cout << "[+] map_tf : ["
    //         << map_tf.getOrigin().x() << ", "
    //         << map_tf.getOrigin().y() << ", "
    //         << map_tf.getOrigin().z() << "] " << std::endl;

    // std::cout << "[+] EOT_tf : ["
    //         << EOT_tf.getOrigin().x() << ", "
    //         << EOT_tf.getOrigin().y() << ", "
    //         << EOT_tf.getOrigin().z() << "] " << std::endl;

    // std::cout << "[+] output_tf : ["
    //         << output_tf.getOrigin().x() << ", "
    //         << output_tf.getOrigin().y() << ", "
    //         << output_tf.getOrigin().z() << "] " << std::endl;
  // ed: pitch isn't used

  std::string parsed_string ="{to_trajectory_id="\
                             + std::to_string(to_trajectory_id)\
                             + ",relative_pose={translation={"\
                             + std::to_string(output_tf.getOrigin().x()) + ","\
                             + std::to_string(output_tf.getOrigin().y()) + ","\
                             + "0" + \
                             + "},rotation={"\
                             + std::to_string(roll) + ","\
                             + std::to_string(0) + ","\
                             + std::to_string(yaw) + ",}}}";
   
   std::cout << parsed_string << std::endl;
  
  std::string s;
  ::ros::NodeHandle n;
  n.param<std::string>("parsed_string",s," ");
  n.setParam("parsed_string",parsed_string);
  
}

int main(int argc, char** argv) {
  google::InitGoogleLogging(argv[0]);
  google::SetUsageMessage(
      "\n\n"
      "Convenience tool around the start_trajectory service. This takes a Lua "
      "file that is accepted by the node as well and starts a new trajectory "
      "using its settings.\n");
  google::ParseCommandLineFlags(&argc, &argv, true);
  
  CHECK(!FLAGS_configuration_directory.empty())
      << "-configuration_directory is missing.";
  CHECK(!FLAGS_configuration_basename.empty())
      << "-configuration_basename is missing.";

  ::ros::init(argc, argv, "cartographer_record_initial_pose");
  ::ros::start();

  // ed: node handler added
  ::ros::NodeHandle nh;
  recordInitialPose();

  //::ros::spin();
  //::ros::shutdown();

  return 0;

}
